#include "CollisionSolving.h"
#include "Sphere.h"
#include "Eitoboru.h"

// Thanks to Joe van den Heuvel, Miles Jackson for their excellent paper
// http://www.gamasutra.com/view/feature/3015/pool_hall_lessons_fast_accurate_.php
bool CollisionSolving::SphereSphere( Sphere *b1, Sphere *b2 )
{
    core::vector3df v1 = b1->GetVelocity(), v2 = b2->GetVelocity();
    core::vector3df p1 = b1->GetPosition(), p2 = b2->GetPosition();

    double dist = p2.getDistanceFrom( p1 );
    double sr = b2->GetRadius() + b1->GetRadius();
    dist -= sr;

    core::vector3df v = v1 - v2;
    
    // Lazy test, length of the movement vector should always at least reach the other sphere
    if( v.getLength() < dist )
    {
        return false;
    }

    // Normalize velocity vector
    core::vector3df n = core::vector3df( v ).normalize();

    // Find the middle of the 2 positions
    core::vector3df c = p2 - p1;

    // Angle between N and C
    double d = n.dotProduct( c );

    // Lazy test, A should be moving towards B
    if( d <= 0.0 )
    {
        return false;
    }

    double lc = c.getLength();
    double f = (lc * lc) - (d * d);

    // Escape test, the closest point should be more than the sum of radii
    double srsr = sr * sr;

    if( f >= srsr )
    {
        return false;
    }

    double t = srsr - f;

    // Find third side of triangle between the both
    if( t < 0 )
    {
        return false;
    }

    if( v.getLength() < d - sqrt( t ) )
    {
        return false;
    }

    return true;
}

bool CollisionSolving::SpherePlane( Sphere *s, Plane *p )
{
    core::vector3df n    = p->GetRotation();
    core::vector3df pos  = p->GetPosition();
    core::vector3df spos = s->GetPosition();
    
    // Project centre of sphere onto plane p
    float t = -((n.dotProduct( spos ) + pos.dotProduct( -n )) / n.getLengthSQ());
    core::vector3df proj = spos + n * t;

    // Check distance, should be less than or equal 
    if( proj.getDistanceFrom( spos ) <= s->GetRadius() )
    {
        static int i = 0;
        printf( "%ith collision\n", i );
    }

    return true;
}

void CollisionSolving::Handle( Sphere *b1, Sphere *b2 )
{
    core::vector3df bv1 = b1->GetVelocity(), bv2 = b2->GetVelocity();
    float m1 = b1->GetMass(), m2 = b2->GetMass();

    core::vector3df n = (b1->GetPosition() - b2->GetPosition()).normalize();
    
    float o = (2.0 * (bv1.dotProduct( n ) - bv2.dotProduct( n ))) / (m1 + m2);

    b1->SetVelocity( bv1 - o * m2 * n ); 
    b2->SetVelocity( bv2 + o * m1 * n );
}

void CollisionSolving::Solve( float t )
{
    for( int i = 0 ; i < 15 ; ++i )
    {
        Sphere *b1 = g_Balls[i];

        if( b1 )
        {
            // Check planecollider
            SpherePlane( b1, g_Plane );

            // Check balls
            for( int n = 0 ; n < 15 ; ++n )
            {
                Sphere *b2 = g_Balls[n];

                if( b2 )
                {
                    if( SphereSphere( b1, b2 ) )
                    {
                        Handle( b1, b2 );
                    }
                }
            }

            // Check pockets
            for( int n = 0 ; n < 6 ; ++n )
            {
                Sphere *b2 = g_Pockets[n];

                if( SphereSphere( b1, b2 ) )
                {
                    delete g_Balls[i];
                    g_Balls[i] = 0;
                    break;
                }
            }
        }
    }
}

void CollisionSolving::PreventTableWalkthrough( void )
{
	core::aabbox3df tableBox = g_Table->getTransformedBoundingBox();
	core::vector3df playerPos = g_Camera->getPosition();
	playerPos.Y = -8;

	if(tableBox.isPointInside(playerPos))
	{
		core::vector3df goingTowards = g_Camera->getTarget();
		goingTowards.Y = 0;
		goingTowards.normalize();
		g_Camera->setPosition( g_Camera->getPosition() - goingTowards*0.2 );
	}
}